﻿using System;

namespace new_problem
{
    class LevenbergMarquardt
    {
        public static Double[,] calcLevenbergMarquardt(double[,] g, double[] x, double[] y, DataBuffer current)
        {
            double eps = 0.01;
            double gamma = 5.0; // gamma should be > 0 
            Double[,] z0 = DataGen.StartApproximation(); // initial approximation
            Double[,] A = DataGen.GetFieldGrid(x, y, z0);
            Double[,] dA = DataGen.GetFieldDerivative(x, y, z0);
            Double[,] dAt = MiscUtils.TransposeMatrix(dA);
            Double[,] zk = new double[DataGen.N + 1, DataGen.N + 1];
            double alpha = 0.01; // 2nd regularization coef

            double nrm = MiscUtils.Norm(g);
            double err = 1.0;
            int k = 0;
            while (err > eps && k < 10000)
            {
                Double[,] inverseMultiplier = MiscUtils.TikhonovRegularization(MiscUtils.MultiplyMatrix(dAt, dA), alpha);
                MiscUtils.InvertMatrix(inverseMultiplier); // получаем обратную матрицу  от (A'(Uk)* A'(Uk) + alpha*I)

                Double[,] secondMultiplier = MiscUtils.MultiplyMatrix(inverseMultiplier,dAt);
                Double[,] multiplication = MiscUtils.MultiplyMatrix(secondMultiplier, MiscUtils.SubstractMatrix(g,A));

                for (int i = 0; i <= DataGen.N; i++)
                    for (int j = 0; j <= DataGen.N; j++)
                        zk[i, j] = z0[i, j] - gamma*multiplication[i, j];
                z0 = zk;
                A = DataGen.GetFieldGrid(x, y, z0);
                dA = DataGen.GetFieldDerivative(x, y, z0);
                dAt = MiscUtils.TransposeMatrix(dA);
                err = MiscUtils.Norm(MiscUtils.SubstractMatrix(A, g)) / nrm;
                k++;
                Console.WriteLine("LM: " + k + ": " + err + ", alpha: " + alpha);
            }
            current.LM = k;
            current.LMErr = err;
            return zk;
        }
    }
}
